#include <robot/robot.h>

#include <robot/pacohead/cheadclient.h>
#include <blackbox/connect.h>

namespace buola { namespace robot { namespace pacohead {

CHeadClient::CHeadClient()
    :   BBox("head client")
    ,   iJoints(this,"joints in",7)
    ,   iStatus(this,"head status")
{
}

CHeadClient::~CHeadClient()
{
}

void CHeadClient::LookAt(const mat::CVec3d &pPos,EMode pMode)
{
    using namespace pacohead;

    CHeadPose lPose;
    GetPose(lPose);
    
    mat::CVec3d lV=pPos;
    if(pMode&LOOKAT_IMAGECOORDS)
        lV=lPose.CamerasToHead(pPos);

    if(pMode&LOOKAT_EYESONLY)
    {
        mat::CVec3d lEV=lPose.HeadToCameras(lV);
        lPose.EyesTilt()+=atan2(-lEV.y(),lEV.z());
        double lDistYZ=sqrt(sq(lEV.y())+sq(lEV.z()));
        lPose.EyesRight()=-atan2(lEV.x()-46.5,lDistYZ);
        lPose.EyesLeft()=-atan2(lEV.x()+46.5,lDistYZ); 

        lPose.Normalize();
    }
    else //if(pMode&LOOKAT_SMOOTH)
    {
        lPose.InvKinematics(lV);
    }

    SetGoal(lPose);
}

void CHeadClient::Init(const blackbox::CBoxProxy &pBox)
{
    mHead.reset(new blackbox::CBoxProxy(pBox));
    bb_connect(pBox["joints out"],iJoints);
    bb_connect(pBox["status out"],iStatus);
}

void CHeadClient::GetPose(std::vector<double> &pValues)
{
    PullAtomicInputs();
    pValues.clear();
    for(int i=0;i<7;i++)
        pValues.push_back(iJoints.Get(i));
}

void CHeadClient::GetPose(CHeadPose &pPose)
{
    std::vector<double> lJoints;
    GetPose(lJoints);
    pPose=CHeadPose(lJoints);
}

void CHeadClient::SetGoal(const mat::CVec3d &pPos)
{
    CHeadPose lPose;
    lPose.InvKinematics(pPos);
    SetGoal(lPose);
}

void CHeadClient::SetGoal(const CHeadPose &pPose)
{
    std::array<double,7> lPose;
    for(int i=0;i<7;i++)
        lPose[i]=pPose[i];
    
    mHead->Call<void(std::array<double,7>)>("set joints",lPose);
}

void CHeadClient::SetGoal(const std::vector<double> &pJoints)
{
    SetGoal(CHeadPose(pJoints));
}

void CHeadClient::SetGoal(int pJoint,double pValue)
{
    CHeadPose lPose;
    GetPose(lPose);
    lPose[pJoint]=pValue;
    SetGoal(lPose);
}

void CHeadClient::Stop()
{
    mHead->Call<void()>("stop");
}

void CHeadClient::DoReset()
{
    mHead->Call<void()>("doreset");
}

bool CHeadClient::IsMoving()
{
    PullAtomicInputs();
    return iStatus.Get();
}

void CHeadClient::WaitForStop()
{
    sleep(120_ms);
    while(IsMoving())
        sleep(10_ms);
}

/*namespace pacohead*/ } /*namespace robot*/ } /*namespace buola*/ }
